"""  
需求：
流程：
    1.导包；
    2.初始化ROS2客户端；
    3.自定义节点类；
                  
    4.调用spain函数，并传入节点对象；
    5.资源释放。


"""
#1.导包；
import rclpy
from rclpy.node import Node
from tf2_ros.transform_listener import TransformListener
from tf2_ros.buffer import Buffer
import tf_transformations
from rclpy.time import Time
from rclpy.time import Duration

#3.自定义节点类；
class TFListener(Node):
    def __init__(self):
        super().__init__("transform_listener_node")
        self.buffer=Buffer()
        self.tl=TransformListener(self.buffer,self)
        self.timer=self.create_timer(1.0,self.get_transform)
    def get_transform(self):
        try:
            result=self.buffer.lookup_transform("base_link","bottle_link",
                            Time(seconds=0),Duration(seconds=5.0))
            translation=result.transform.translation
            ratation_euler=tf_transformations.euler_from_quaternion(
                 [result.transform.rotation.x,
                  result.transform.rotation.y,
                  result.transform.rotation.z,
                  result.transform.rotation.w
                 ])
            self.get_logger().info(f"平移:{translation},旋转四元数:{result.transform.rotation}欧拉角:{ratation_euler}")
        except Exception as e:
                self.get_logger().warn(f"不能获取坐标变化，原因：{str(e)}")
def main():
    #2.初始化ROS2客户端；
    rclpy.init()
    #4.调用spain函数，并传入节点对象；
    rclpy.spin(TFListener())
    #5.资源释放。
    rclpy.shutdown()

if __name__=='__main__':
    main() 